﻿#ifndef MINGZHIMOTOR_H
#define MINGZHIMOTOR_H

#include "can/candevice.h"
#include "motordriver.h"

// MOONS'
//待提供pulse_per_round

class MingzhiMotor : public MotorDriver {
public:
    MingzhiMotor( int pulse_per_round, double length_per_pulse, int left_direction, int right_direction, long acce, long dece );
    ~MingzhiMotor() override;

    //一、节点NMT模式切换为operation
    //二、电机初始化
    //三、电机控制参数设置
    //四、电机参数读取

    //初始化
    void DataFramesMotorInit( CanDataFrame* frames, uint8& i ) override;
    void DataFramesMotorSpeed( CanDataFrame* frames, uint8& i, double speedl, double speedr ) override;
    void DataFramesMotorRequest( CanDataFrame* frames, uint8& i ) override;

    QString RecDataParse( CanDataFrame* frames, uint8 cnt ) override;

    DMotorFeedBackPara RetMotorFeedbackParam() override;
    void DataFramesMotorEnable( CanDataFrame* frames, uint8& i ) override;   //左右伺服电机重新使能
    void DataFramesMotorDisable( CanDataFrame* frames, uint8& i ) override;  //左右伺服电机停止失能
    void DataFramesAccDecUpdate( CanDataFrame* frames, uint8& i, double& acc, double& dec ) override;
    void DataFramesStopMotor( CanDataFrame* frames, uint8& i ) override;        //全部伺服电机设置停止模式
    void DataFramesGetDriveErrInfo( CanDataFrame* frames, uint8& i ) override;  //获取驱动的错误信息
    void ResetWatchDog( CanDataFrame* frames, uint8& i ) override;
    void AlarmClean( CanDataFrame* frames, uint8& i ) override;  //报警清除

protected:
    QString DataParse( NODE_INDEX index, uint8 ( &array )[ 8 ], SMotorFeedBackPara& para ) override;
    QString MotorErrInfo( NODE_INDEX index, int err_info );

private:
// dictionary object
#define MONITOR_TIMER 2000

    enum DO_INDEX {
        DO_READ_SPEED       = 0x606C,  //获取伺服速度，INTEGER32 Pulses/s
        DO_READ_POS         = 0x6064,  //获取伺服位置，INTEGER32 Pulses
        DO_READ_ERR         = 0x603F,  //获取故障信息
        DO_CONTROL_BYTE     = 0x6040,  //控制字设置PDS(POWER DRIVE SYSTEM)状态
        DO_PULSES_PER_ROUND = 0x2A90,  //每转所需脉冲数 10000  pulses/rev 重启后生效
                                       //第二编码器分辨率 10000 pulses/mm 立即生效
        DO_WATCH_DOG   = 0x2060,       //看门狗
        DO_ALARM_CLEAR = 0x2006,       //报警清除
    };
    enum ALARM_CLEAR_ACTION {
        ACAF = 0X55,
        ACAT = 0XAA,
    };

    enum WD_SUB_INDEX {
        StartOff                             = 0x01,
        MonitorTime                          = 0x03,
        DepartureEvent                       = 0x04,
        WatchdogMotorActionAfterInterruption = 0x05,
    };

    enum WD_START_OFF {
        Off   = 0x00,
        Start = 0x01,
    };

    enum WD_MOTOR_ACTION {
        StopAndDisable = 0x00,  //减速停止并去使能
        StopAndEnable  = 0x01   //减速停止、保持使能状态
    };

    enum WD_DEPARTURE_EVENT { RPDO1 = 0x01, RPDO2 = 0x02, RPDO3 = 0x04, RPDO4 = 0x08, SYNC = 0x10 };

    enum ERR_CODE {
        ERR00 = 0x0000,  //无异常
        ERR01 = 0xFF01,  //驱动器过流
        ERR02 = 0xFF02,  //驱动器过压
        ERR03 = 0xFF03,  //驱动器处理器过温
        ERR04 = 0xFF04,  //预留功能
        ERR05 = 0xFF05,  //驱动器内部电压错误
        ERR06 = 0xFF06,  //位置误差超限
        ERR07 = 0xFF07,  //电机编码器未连接
        ERR08 = 0xFF08,  //全闭环混合偏差超限
        ERR09 = 0xFF09,  //第二编码器未连接
        ERR10 = 0xFF0A,  //再生电势泄放失败
        ERR11 = 0xFF0B,  //安全转矩禁止中（STO）
        ERR12 = 0xFF0C,  //预留功能
        ERR13 = 0xFF0D,  // FPGA错误
        ERR14 = 0xFF0E,  //参数读取失败
        ERR15 = 0xFF0F,  //绝对值编码器多圈错误
        ERR16 = 0xFF10,  //电机堵转保护
        ERR17 = 0xFF11,  //驱动器功率模块过温
        ERR18 = 0xFF31,  //正转禁止限位
        ERR19 = 0xFF32,  //反转禁止限位
        ERR20 = 0xFF33,  //正转禁止限位及反转禁止限位
        ERR21 = 0xFF34,  //电机重载状态
        ERR22 = 0xFF35,  //在电机未使能时命令其运转
        ERR23 = 0xFF36,  //驱动器低压
        ERR24 = 0xFF37,  //调用的Q程序段为空
        ERR25 = 0xFF38,  //电机速度超过限值
        ERR26 = 0xFF39,  //驱动器主回路电源输入缺相
        ERR27 = 0xFF3A,  //数字量输入紧急停止
        ERR28 = 0xFF3B,  //绝对值编码器欠压
        ERR29 = 0xFF3C,  //绝对位置丢失
        ERR30 = 0xFF3D,  //绝对位置溢出
        ERR31 = 0xFF3E,  //电机过温
        ERR32 = 0xFF3F,  //驱动器欠压
        ERR33 = 0xFF41,  //参数保存失败
        ERR34 = 0xFF42,  // I/O信号功能复用
        ERR35 = 0xFF43,  //总线看门狗触发
        ERR36 = 0xFF46,  //用户手册没有，待确定
        ERR37 = 0xFFFF,  //其他错误
    };

    // uint8 can_operation_disable = 0x02;//伺服can操作模式失能(NMT进入stop)暂时屏蔽
    DMotorFeedBackPara mfb_para;

    long speedL;  //速度
    long speedR;  //速度
    // long acc;       //加速度
    // long dec;       //减速度
    // int left_direction  = 0;  // 左轮方向修补  根据电机实际安装决定
    // int right_direction = 0;  // 右轮方向修补
    double length_per_pulse;  //一个脉冲轮子对应走过的距离
    long   acce, dece;
    int    pulse_per_round;

    //电机使能向0x6040：0x06、0x07、0x010F
    uint8 motor_controlByte_00[ 8 ]  = { 0x2B, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00 };  // quick stop（电机失能）
    uint8 motor_controlByte_06[ 8 ]  = { 0x2B, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00 };  //准备好启动（电机失能）
    uint8 motor_controlByte_07[ 8 ]  = { 0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00 };  //启动驱动
    uint8 motor_controlByte_10F[ 8 ] = { 0x2B, 0x40, 0x60, 0x00, 0x0F, 0x01, 0x00, 0x00 };  //激活操作模式(电机使能)
    uint8 motor_controlByte_0F[ 8 ]  = { 0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00 };  //启动运动

    //设置速度模式
    uint8 speed_mode[ 8 ] = { 0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00 };  // 设置伺服速度模式
    //设置运行参数:向0x60FF、0x6083、0x6084写入目标速度、加速度和减速度
    uint8 set_speed_value[ 4 ] = { 0x23, 0xFF, 0x60, 0x00 };  // 设定速度模式下速度值，如:设置速度为1200rpm（200000<0x30D40>/10000=20rps，乘60s/m)
    uint8 set_drive_accec[ 4 ] = { 0x23, 0x83, 0x60, 0x00 };  // 设定加速度         ，如:设置加速度为12000rpm（2000000<0x1E8480>/10000=200rps，乘60s/m)
    uint8 set_drive_dece[ 4 ]  = { 0x23, 0x84, 0x60, 0x00 };  // 设定减速度         ，如:设置减速度为12000rpm（（2000000<0x1E8480>/10000=200rps，乘60s/m)
    //启动停止电机
    // uint8* start_motor  = motor_controlByte_0F;     //启动运动
    // uint8* stop_motor   = motor_controlByte_10F;    //停止运动
    uint8* start_driver = motor_controlByte_07;  //启动驱动
    //伺服电机使能失能
    uint8* enable_motor  = motor_controlByte_0F;  //电机使能
    uint8* disable_motor = motor_controlByte_06;  //电机失能
    //看门狗（节点保护）
    // uint8 watchdog_monitor_time[8]                  = {0x2B, 0x60, 0x20, 0x03, 0xE8, 0x03, 0x00, 0x00};//1、设置监控时间为1s,(0x03E8=1000)
    // uint8 watchdog_departureEvent[8]                = {0x2B, 0x60, 0x20, 0x04, 0x10, 0x00, 0x00, 0x00};//2、设置触发事件，以SYNC为触发条件
    // uint8 watchdog_motorActionAfterInterruption[8]  = {0x2B, 0x60, 0x20, 0x05, 0x01, 0x00, 0x00, 0x00};//3、设置通讯中断后电机动作通讯中断后，减速停止且使能
    // uint8 watchdog_start[8]                         = {0x2B, 0x60, 0x20, 0x01, 0x01, 0x00, 0x00, 0x00};//4、启动看门狗
    // uint8 watchdog_stop[8]                          = {0x2B, 0x60, 0x20, 0x01, 0x00, 0x00, 0x00, 0x00};//5、关闭看门狗

    // uint8 get_cur_speed[8]  = {0x40, 0x6C, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00};  // 获取伺服速度
    // uint8 get_cur_pos[8]    = {0x40, 0x64, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00};  // 获取伺服位置
    // uint8 get_err_info[8]   = {0x40, 0x3F, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00};  // 获取故障信息

    void CanDataFrameNMTMode( CanDataFrame& frame, NMT_MODE mode );  // 节点进入NMT operation模式

    void CanDataFrameEnableMotor( CanDataFrame& frame, uint8 drive_id );   // 电机使能
    void CanDataFrameDisableMotor( CanDataFrame& frame, uint8 drive_id );  // 电机失能
    void CanDataFrameStartDriver( CanDataFrame& frame, uint8 drive_id );   // 启动驱动
    void CanDataFrameStartMove( CanDataFrame& frame, uint8 drive_id );     // 启动运动
    void CanDataFrameStopMove( CanDataFrame& frame, uint8 drive_id );      // 停止运动

    void CanDataFrameControlWord( CanDataFrame& frame, NODE_INDEX index, PDS_STA mode );  // 伺服电机状态设置
    void CanDataFrameSpeedMode( CanDataFrame& frame, uint8 drive_id );                    // 伺服电机设置速度模式

    //看门狗功能配置（节点保护）
    void CanDataFrameWatchDogSetMonitorTimer( CanDataFrame& frame, uint8 drive_id );                  // 1、设置监控时间，设置监控时间为5s,(0x1388=5000)
    void CanDataFrameWatchDogSetDepartureEvent( CanDataFrame& frame, uint8 drive_id );                // 2、设置触发事件，以SYNC为触发条件
    void CanDataFrameWatchDogSetMotorActionAfterInterruption( CanDataFrame& frame, uint8 drive_id );  // 3、设置通讯中断后电机动作
    void CanDataFrameWatchDogStart( CanDataFrame& frame, uint8 drive_id );                            // 4、启动看门狗
    void CanDataFrameWatchDogStop( CanDataFrame& frame, uint8 drive_id );                             // 5、关闭看门狗

    void CanDataFrameSetSpeedValue( CanDataFrame& frame, uint8 drive_id, long value );  // 伺服电机设置速度值
    void CanDataFrameSetAcce( CanDataFrame& frame, uint8 drive_id, long value );        // 伺服电机设置加速度
    void CanDataFrameSetDece( CanDataFrame& frame, uint8 drive_id, long value );        // 伺服电机设置减速度

    void CanDataFrameSYNC( CanDataFrame& frame );  // 同步喂狗

    void CanDataFrameRquestMotorCurSpeed( CanDataFrame& frame, uint8 drive_id );                 // 获取伺服速度
    void CanDataFrameRquestMotorPosition( CanDataFrame& frame, uint8 drive_id );                 // 获取伺服位置
    void CanDataFrameRquestMotorFaultCode( CanDataFrame& frame, uint8 drive_id );                // 获取故障信息
    void CanDataFrameAlarmClean( CanDataFrame& frame, uint8 drive_id, ALARM_CLEAR_ACTION act );  //报警清除

    // uint8      set_node_proetc_time[ 4 ]                = { 0x2B, 0x0C, 0x10, 0x00 };                          // 100C 是节点保护时间(单位:ms)
    // uint8      set_node_proetc_factor[ 4 ]              = { 0x2F, 0x0D, 0x10, 0x00 };                          // 100D 是节点保护因子
    // int        left_drive_err_info                      = 0;
    // int        right_drive_err_info                     = 0;
    // int        left_direction                           = 0;  // 左轮方向修补  根据电机实际安装决定
    // int        right_direction                          = 0;  // 右轮方向修补
    // int        calc_left_puls = 0, calc_right_puls = 0;
    // double     calc_left_reality_speed = 0, calc_right_reality_speed = 0;
    // double     send_left_speed = 0, send_right_speed = 0;
    // bool       is_open_can           = false;  // can设备是否打开
    // bool       is_lift_pos_mode_done = false;
    // bool       is_turn_pos_mode_done = false;
    // bool       is_connect            = false;
    // bool       is_heart_beat_active  = false;
    // long       acce, dece;
    // QTimer*    lose_connect_timer = nullptr;  //断线超时计时器
    // QTimer*    timer50ms          = nullptr;  // UPDATE 2022.11.12 can.h 定时参数改全局变量timer50ms
    // QTimer*    heart_beat_timer   = nullptr;  //心跳保护超时计时器
    // CanDevice* canDevice          = nullptr;
};

#endif  // MINGZHIMOTOR_H
